#include "glbase/glflat.h"
#include "glbase/glnavig.h"	

//#define DEBUG
#define N_TRIANG 4
#define MARK_SIZE 50.0f
#define N_POINTS 100
#define N_STATE 15
#define N_CORRECT 3
#define N_GPS_UPDATE 100
#define N_TEST 10000
#define N_MODELS 3
#define BETA_A 0.01
#define BETA_W 0.01

BOOL
	updateGraph=TRUE,
	simGraph=FALSE,
	pauseOnGraph=TRUE;

int i=0,j=0,k=0;
BOOL correct;
int row,col,counter=0;
double dt,simTime= 0,maxSimTime = 20;

double	
		euler[3],Wnb_b[3],
		r_gps[3],v_gps[3],
		input_a[3],input_w[3],
		measure_var[N_CORRECT]={pow2(20/A_EARTH),pow2(20/A_EARTH),400},
		noise_var[N_STATE]={
						1e-4,1e-4,1e-4, // dev r
						1e-4,1e-4,1e-4, // dev v
						1e-4,1e-4,1e-4, // dev q
						1e-4,1e-4,1e-4, // dev a
						1e-4,1e-4,1e-4}; // dev w

enum modes {SIM_GRAPH,DATA_INFO,STATE_INFO,ERROR_INFO,VAR_INFO};
STR  modeNames[]={_T("GRAPH"),_T("DATA"),_T("STATE"),_T("ERROR"),_T("VAR")};
const int modeCount=sizeof(modeNames)/sizeof(modeNames[0]);

BOOL
	mode=1,
	initOK=FALSE,
	light=TRUE,
	fill=TRUE,
	pause=FALSE,
	pp=FALSE,
	kup=FALSE,
	kdp=FALSE,
	tabp=FALSE,
	lp=FALSE,
	retp=FALSE,
	sp=FALSE;
GLfloat
	z=-3.0f,
	LightAmbient[]= { 0.5f, 0.5f, 0.5f, 1.0f },
	LightDiffuse[]= { 1.0f, 1.0f, 1.0f, 1.0f },
	LightPosition[]= { 0.0f, 0.0f, 2.0f, 1.0f };



class Sim:public FlatApplication
{
	Font fontSmall,fontMatrix;
	INS ins;
	INSError insError;

	FlatGraph fg;
	BOOL initSim();
	void updateSim();
	void drawGeoPosition(
		float cx,float cy ,float cz,
		float lat, float lon, float alt,
		float x, float y, float z,
		float l, float t,
		int markColor = 0,
		int traceColor = 0,
		int frameColor = 0,
		int projectColor = 0,
		BOOL cercle = TRUE, int n = 32);
	void drawObject();

	BOOL create();
	void draw2D();
	void draw3D();
	BOOL glCreate();
	void update();
	void glDestroy();
	BOOL listen(WORD id, WORD code, const void *info);
	
} sim;

BOOL Sim::listen(WORD id, WORD code, const void *info)
{
	return FALSE;
}
BOOL Sim::create ()				
{
	FlatApplication::create();
	name=_T("Sim");

	format.precision=3;

	display.width=1024;
	display.height=768;
	display.bpp=32;

	fontSmall.setFace(T("Courier New"));
	fontSmall.height=-12;

	fontMatrix.setFace("Courier New"));
	fontMatrix.height=-10;

	fg.bgColor=INT_GRAY;
	fg.graph.nGridX=5;
	fg.graph.nGridY=5;
	fg.graph.margin=10;
	fg.graph.marginChannel.set(15,100);
	fg.graph.font=&fontSmall;
	fg.graph.formatY.precision=3;
	fg.graph.formatY.exposant=TRUE;
	fg.setBounds(10,50,float(display.width-20),float(display.height-60));
	fg.margin.set(5,5);
	fg.fontColor=INT_BLACK;
	fg.format.precision=6;
	fg.channelRead=7;

	fg.graph.add( _T("time"),INT_WHITE,0,&simTime);
	fg.graph.add( _T("lat"),INT_DBLUE,1,&ins.r_e[GEO_LAT]);
	fg.graph.add( _T("lon"),INT_BLUE,1,&ins.r_e[GEO_LON]);
	fg.graph.add( _T("alt"),INT_WBLUE,1,&ins.r_e[GEO_ALT]);
	fg.graph.add( _T("vnorth"),INT_DRED,1,&ins.v_n[NED_NORTH]);
	fg.graph.add( _T("veast"),INT_RED,1,&ins.v_n[NED_EAST]);
	fg.graph.add( _T("vdown"),INT_WRED,1,&ins.v_n[NED_DOWN]);
	fg.graph.hideAll();

	add(&fg);

	return initSim();			
}
BOOL Sim::glCreate ()
{
	glClearDepth(1.0f);
	glDepthFunc(GL_LEQUAL);	
	glHint(GL_PERSPECTIVE_CORRECTION_HINT, GL_NICEST);
	createLight(GL_LIGHT1,LightAmbient,LightDiffuse,LightPosition);
	fontSmall.create(window.hDC);
	fontMatrix.create(window.hDC);
	FlatApplication::glCreate();
	return TRUE;												
}
void Sim::glDestroy()
{
	FlatApplication::glDestroy();
	fontSmall.destroy();
	fontMatrix.destroy();
}
void Sim::update()
{
	dt=drawTime;
	counter++;
	BOOL *keys=keyboard.keys;
	updated=TRUE;

	if (keys [VK_ESCAPE])
	{
		terminate();
	}
	if (keys [VK_F1])
	{
		toggleFullscreen();
		keys [VK_F1] = FALSE;
	}
	if (keys ['L'])					// Is L Being Typed?
	{
		if(!lp)
		{
			lp=TRUE; // lp Becomes TRUE
			light=!light; // Toggle Light TRUE/FALSE
			setLight(light);
		}
	}
	else
	{
		lp = FALSE;
	}
	if (keys [VK_SPACE])					// Is Space Being Typed?
	{
		if(!sp)
		{
			sp=TRUE; // fp Becomes TRUE
			fill=!fill;
			fillPolygon(fill);
		}
	}
	else
	{
		sp=FALSE;
	}
	if (keys['P'])					// Is Space Being Typed?
	{
		if(!pp)
		{
			pp=TRUE; // fp Becomes TRUE
			pause=!pause;
		}
	}
	else
	{
		pp=FALSE;
	}
	if (keys [VK_TAB])			
	{
		if(!tabp)
		{
			tabp=TRUE; // fp Becomes TRUE
			if(mode<2)
				mode++;
			else
				mode=0;
		}
	}
	else
	{
		tabp=FALSE;
	}
	if (keys [VK_RETURN])			
	{
		if(!retp)
		{
			retp=TRUE; // fp Becomes TRUE
			initSim();
		}
	}
	else
	{
		retp=FALSE;
	}
	if (keys ['1'])			
	{
		mode=0;
	}
	if (keys ['2'])			
	{
		mode=1;
	}
	if (keys ['3'])			
	{
		mode=2;
	}
	if (keys ['4'])			
	{
		mode=3;
	}
	if (keys ['5'])			
	{
		mode=4;
	}
	if (keys ['6'])			
	{
		mode=5;
	}
	if (keys ['7'])			
	{
		mode=6;
	}
	if (keys ['8'])			
	{
		mode=7;
	}
	if (keys ['9'])			
	{
		mode=8;
	}
	if (keys ['0'])			
	{
		mode=9;
	}
	if(pause)
		return;
	updateSim();
}
BOOL Sim::initSim()
{
	double
		euler0[3]={ 0, 0, 0},
		w0[3]={ 0, 0, 0},
		r0[3]={ 0, 0, 0},
		v0[3]={ 0, 0, 0},
		a0[3]={ 0, 0, 0};

	copyVector(a0,input_a);
	copyVector(w0,input_w);

	ins.inite(r0,v0,euler0,input_a,input_w);
	insError.init(&ins,BETA_A,BETA_W);

	simTime=0;
	simGraph=TRUE;
	fg.graph.clear();

	pause=FALSE;
	
	return TRUE;
}
void Sim::updateSim()
{
	if(simGraph&&maxSimTime>0)
	{
		simTime+=dt;
		if(updateGraph)
		{
			if(maxSimTime>simTime)
			{
				fg.graph.addPoint();
				fg.graph.update();
			}
			else
			{
				simGraph=FALSE;
				if(pauseOnGraph)
					pause=TRUE;
			}
		}
		else
		{
			if(maxSimTime>simTime)
				fg.graph.addPoint();
			else
			{
				fg.graph.update();
				simGraph=FALSE;
				if(pauseOnGraph)
					pause=TRUE;
			}
		}
	}
	ins.transRotation(Wnb_b,input_w);
	ins.update(input_a,input_w,dt);
	insError.predict(dt,noise_var);
	insError.correct(ins.r_e,measure_var);
}
void Sim::draw3D()
{
	/*
	switch(mode)
	{
		case 0:
		{
		}
		break;
	}
	*/
}
void Sim::draw2D()
{
	fg.visible=FALSE;
	if(pause)
	{
		font.color=INT_RED;
		font.x= 10;
		font.y= 15;
		glPrint(&font,_T("* PAUSED *"));
	}
	font.color=INT_WGRAY;
	font.x=120;
	font.y=15;
	glPrint(&font,_T("sample time=%4.1f ms, run time=%6.3f s, graph remain=%6.3f s"),drawTime*1e3,runTime,maxSimTime-simTime);
	font.x=10;
	font.y=40;
	for(i=0;i<modeCount;i++)
	{
		if(i!=mode)
			font.color=INT_GRAY;
		else
			font.color=INT_YELLOW;
		glPrint(&font,_T("%d.%s"), i+1, modeNames[i]);
		font.x+=120;
	}
	font.x=10;
	font.y=40;
	switch(mode)
	{
		case SIM_GRAPH:
		{
			fg.visible=TRUE;
		}
		break;
		case DATA_INFO:
		{
			font.color=INT_YELLOW;
			font.y=100;
			glPrint(&font,_T("Position:"));
			font.y+=18;
			font.color=INT_WYELLOW;
			glPrint(&font,_T("x:%13.4f m"), ins.re[0]);
			font.y+=18;
			glPrint(&font,_T("y:%13.4f m"), ins.re[1]);
			font.y+=18;
			glPrint(&font,_T("z:%13.4f m"), ins.re[2]);
			font.color=colorMix(INT_WYELLOW,INT_ORANGE);
			font.y+=25;
			glPrint(&font,_T("lat:% 11f rad"), ins.r_e[GEO_LAT]);
			font.y+=18;
			glPrint(&font,_T("lon:% 11f rad"), ins.r_e[GEO_LON]);
			font.y+=18;
			glPrint(&font,_T("alt:% .3e m"), ins.r_e[GEO_ALT]);
			font.color=INT_ORANGE;
			font.y+=25;
			glPrint(&font,_T("map u :%13.3lf"), ins.mapr[MAP_U]);
			font.y+= 18;
			glPrint(&font,_T("map v :%13.3lf"), ins.mapr[MAP_V]);

			font.color=0xFF00FF00;
			font.y+= 30;
			glPrint(&font,_T("Body A [m/s2]:"));
			font.y+= 18;
			font.color=0xFFC0FFC0;
			glPrint(&font,_T("Ax:%+.4e"), ins.a[0]);
			font.y+= 18;
			glPrint(&font,_T("Ay:%+.4e"), ins.a[1]);
			font.y+= 18;
			glPrint(&font,_T("Az:%+.4e"), ins.a[2]);

			font.color=INT_RED;
			font.y+= 30;
			glPrint(&font,_T("Full Nav A [m/s2]:"));
			font.color=INT_WRED;
			font.y+= 18;
			glPrint(&font,_T("Ax:%+.4e"), ins.vn_dot[0]);
			font.y+= 18;
			glPrint(&font,_T("Ay:%+.4e"), ins.vn_dot[1]);
			font.y+= 18;
			glPrint(&font,_T("Az:%+.4e"), ins.vn_dot[2]);
			
			font.color=0xFFFFFF00;
			font.y+= 30;
			glPrint(&font,_T("Inert A [m/s2]:"));
			font.y+= 18;
			font.color=0xFFFFFFB0;
			glPrint(&font,_T("Ax:%+.4e"), ins.aInert[0]);
			font.y+= 18;
			glPrint(&font,_T("Ay:%+.4e"), ins.aInert[1]);
			font.y+= 18;
			glPrint(&font,_T("Az:%+.4e"), ins.aInert[2]);

			font.color=INT_ORANGE;
			font.y+= 30;
			glPrint(&font,_T("Nav V [m/s]:"));
			font.y+= 18;
			font.color=INT_YELLOW;
			glPrint(&font,_T("Vx:% .3e"), ins.v_n[0]);
			font.y+= 18;
			glPrint(&font,_T("Vy:% .3e"), ins.v_n[1]);
			font.y+= 18;
			glPrint(&font,_T("Vz:% .3e"), ins.v_n[2]);

			font.color=INT_DCYAN;
			font.y= window.height-70;
			double y0=font.y;
			glPrint(&font,_T("Wib_b [rad/s]:"));
			font.y+= 18;
			font.color=INT_CYAN;
			glPrint(&font,_T("Wx:% .3e"), ins.Wib_b[0]);
			font.y+= 18;
			glPrint(&font,_T("Wy:% .3e"), ins.Wib_b[1]);
			font.y+= 18;
			glPrint(&font,_T("Wz:% .3e"), ins.Wib_b[2]);

			font.color=INT_BLUE;
			font.x+= 150;
			font.y=y0;
			glPrint(&font,_T("Win_n [rad/s]:"));
			font.y+= 18;
			font.color=INT_WBLUE;
			glPrint(&font,_T("Wx:% .3e"), ins.Win_n[0]);
			font.y+= 18;
			glPrint(&font,_T("Wy:% .3e"), ins.Win_n[1]);
			font.y+= 18;
			glPrint(&font,_T("Wz:% .3e"), ins.Win_n[2]);

			font.color=INT_GREEN;
			font.x+= 150;
			font.y=y0;
			glPrint(&font,_T("Wnb_b[rad/s]:"));
			font.y+=18;
			font.color=INT_WGREEN;
			glPrint(&font,_T("roll :% .3e"), ins.Wnb_b[ROT_ROLL]);
			font.y+=18;
			glPrint(&font,_T("pitch:% .3e"), ins.Wnb_b[ROT_PITCH]);
			font.y+=18;
			glPrint(&font,_T("yaw  :% .3e"), ins.Wnb_b[ROT_YAW]);

			font.color=INT_WGRAY;
			font.x+= 180;
			font.y=y0;
			glPrint(&font,_T("Wie_n[rad/s]:"));
			font.y+=18;
			font.color=INT_WHITE;
			glPrint(&font,_T("Wx:% .3e"), ins.Wie_n[0]);
			font.y+=18;
			glPrint(&font,_T("Wy:% .3e"), ins.Wie_n[1]);
			font.y+=18;
			glPrint(&font,_T("Wz:% .3e"), ins.Wie_n[2]);

			

			font.color=INT_WGRAY;
			font.x=300;
			font.y=100;
			glPrint(&font,_T("Euler[deg]:"));
			font.color=INT_WHITE;
			font.y+= 18;
			glPrint(&font,_T("bank    :%6.1f"), euler[0]*RAD_TO_DEG);
			font.y+= 18;
			glPrint(&font,_T("attitude:%6.1f"), euler[1]*RAD_TO_DEG);
			font.y+= 18;
			glPrint(&font,_T("heading :%6.1f"), euler[2]*RAD_TO_DEG);

			font.y+= 30;
			font.color=INT_WGRAY;
			glPrint(&font,_T("Quaternion:"));
			font.color=INT_WHITE;
			font.y+= 18;
			glPrint(&font,_T("q0:% .3f"), ins.q[0]);
			font.y+= 18;
			glPrint(&font,_T("q1:% .3f"), ins.q[1]);
			font.y+= 18;
			glPrint(&font,_T("q2:% .3f"), ins.q[2]);
			font.y+= 18;
			glPrint(&font,_T("q3:% .3f"), ins.q[3]);

			font.color=INT_MAGENTA;
			font.y+= 30;
			glPrint(&font,_T("R: (det=%6.3f)"),detMatrix(ins.Rbn));
			font.y+= 18;
			font.color=INT_WMAGENTA;
			glPrint(&font,_T("% .3f % .3f % .3f"), ins.Rbn[0][0],ins.Rbn[0][1],ins.Rbn[0][2]);
			font.y+= 18;
			glPrint(&font,_T("% .3f % .3f % .3f"), ins.Rbn[1][0],ins.Rbn[1][1],ins.Rbn[1][2]);
			font.y+= 18;
			glPrint(&font,_T("% .3f % .3f % .3f"), ins.Rbn[2][0],ins.Rbn[2][1],ins.Rbn[2][2]);
		}
		break;
		case STATE_INFO:
		{
			printMatrix(&fontMatrix,&insError.A,_T("A:"),10,60,65,20,&fontSmall,_T("% .2e"));
			printMatrix(&fontMatrix,&insError.K,_T("K:"),10,400,65,20,&fontSmall,_T("% .2e"));
		}
		break;
		case ERROR_INFO:
		{
			printMatrix(&fontSmall,&insError.P.diag(),_T("P diag:"),10,60,65,20,&fontSmall,_T("% .6e"));
			printMatrix(&fontSmall,&insError.error,_T("error:"),310,60,65,20,&fontSmall,_T("% .6e"));
		}
		break;
		case VAR_INFO:
		{
			printMatrix(&fontMatrix,&insError.P,_T("P:"),10,60,65,20,&fontSmall,_T("% .2e"));
			printMatrix(&fontMatrix,&insError.Q,_T("Q:"),10,400,65,20,&fontSmall,_T("% .2e"));
		}
		break;
	}
}

int WINAPI _tWinMain(HINSTANCE hInstance, HINSTANCE hPrevInstance, LPSTR lpCmdLine, int nCmdShow)
{
	return sim.main(hInstance,hPrevInstance,lpCmdLine,nCmdShow);
}/**/